#include <defs.h>
#include <bwio.h>
#include <ts7200.h>
#include <pq.h>
#include <syscalls.h>
#include <string.h>

int Min(int a, int b) {
	return (a < b ? a : b);
}

int SysCreate( sched *s, td *task, int priority, void (*func) () ) {
  // check for error conditions
  if ( priority < 0 || priority >= MAX_PRI ) return -1;
  if ( s->next_task_id == MAX_TASKS ) return -2;

  // set up the task descriptor
  // NOTE: initialize everything - Resource Acquisition Is Initialization!
  int id = s->next_task_id++;
  s->tasks[id].id = id;
  s->tasks[id].sp = 0x01e00000 - STACK_SIZE * id;
  s->tasks[id].spsr = 0x600000d0;
  s->tasks[id].ret = 0xdeadbeef;
  s->tasks[id].mode = SWI;
  s->tasks[id].parent = task;
  s->tasks[id].pri = priority;
  s->tasks[id].state = STATE_READY;
  s->tasks[id].next = NULL;
  s->tasks[id].prev = NULL;
  PqInit( &(s->tasks[id].sendQ) );
  s->tasks[id].msg = NULL;
  s->tasks[id].msg_len = -1;
  s->tasks[id].reply = NULL;
  s->tasks[id].reply_len = -1;
  s->tasks[id].sender_tid = NULL;
  s->tasks[id].event = NULL;
  s->tasks[id].event_len = -1;

  // place into priority queue
  PqPushBack( &(s->queues[priority]), &(s->tasks[id]) );

  // set up the task stack
  int i;
  for (i = 13; i >= 0; i--) {                               // r12-r0
    TdPushRegister( &(s->tasks[id]), 0xdeadbeef );
  }
  TdPushRegister( &(s->tasks[id]), (int)( func ) + CODE_BASE );  // lr

  return id;
}

int SysMyTid( sched *s, td *task ) {
  return task->id;
}

int SysMyParentTid( sched *s, td *task ) {
  if ( task->parent == NULL ) return -1;
  return task->parent->id;
}

void SysPass( sched *s, td *task ) {}

void SysExit( sched *s, td *task ) {
  // notify all tasks on send queue
  while ( !PqEmpty( &task->sendQ ) ) {
    td *sender = PqPopFront( &task->sendQ );
    
    // warn the sender that the transaction is incomplete
    sender->ret = ERROR_TX_INCOMPLETE;

    // ready the sender
    sender->state = STATE_READY;
	  PqPushBack(&s->queues[sender->pri], sender);
  }
  task->state = STATE_DEFUNCT;
}

void SysFingerOfDeath( sched *s, td *task ) {
  // remove all tasks from all priority queues
  int i;
  td *next_task;
  for (i = MAX_PRI; i >= 0; i--) {
    while (!PqEmpty(&(s->queues[i]))) {
      next_task = PqPopFront(&(s->queues[i]));
      next_task->state = STATE_DEFUNCT;
    }
  }
}

int SysSend( sched *s, td *task,
    int tid, char *msg, int msg_len, char *reply, int reply_len ) {	
  // error checking
  if ((msg == NULL && msg_len > 0) || (reply == NULL && reply_len > 0))
		return ERROR_NULL_POINTER;

  if (tid < 0 || tid >= s->next_task_id)
		return ERROR_INVALID_TID;

  td *sender = task;
  td *receiver = &s->tasks[tid];
	
  if (receiver->state == STATE_DEFUNCT)
		return ERROR_TX_INCOMPLETE;
	
  // set fields so that Reply() can copy the reply
  sender->reply = reply;
	sender->reply_len = reply_len;

  // can't overflow either buffer
	int to_copy = Min(receiver->msg_len, msg_len);
	
	if (receiver->state == STATE_SEND_BLOCK) {
    // the receiver has called Receive() and has an empty send queue
		// copy the sent message to the receiver
		MemCopy(receiver->msg, msg, to_copy);
		
		// ready the receiver, set appropriate return value
		*(receiver->sender_tid) = sender->id;
		
    if (msg_len > receiver->msg_len)
      receiver->ret = ERROR_MSG_TRUNCATED;
    else
      receiver->ret = msg_len;
		receiver->state = STATE_READY;
		
		PqPushBack(&s->queues[receiver->pri], receiver);
		
		// block the sender, waiting for Reply()
		sender->state = STATE_REPLY_BLOCK;		
	} else {
    // the receiver has never called Receive() or has a non-empty send queue
    // put the sender on the receiver's send queue
		PqPushBack(&receiver->sendQ, sender);
		
    // set fields so that Receive() can copy the sent message
		sender->msg = msg;
		sender->msg_len = msg_len;

    // block the sender, waiting for Receive()
	  sender->state = STATE_RECEIVE_BLOCK;
	}
	
  // return for now; Reply() fills in the actual return value
  return SUCCESS;
}

int SysReceive(sched *s, td *task, int *tid, char *msg, int len) {	
	// error checking
  if (tid == NULL || (msg == NULL && len > 0))
		return ERROR_NULL_POINTER;

  td *receiver = task;

	if (PqEmpty(&task->sendQ)) {
    // the receiver has an empty send queue
    // set fields so that Send() can copy the sent message and set tid
		receiver->msg = msg;
		receiver->msg_len = len;
    receiver->sender_tid = tid;
		
    // block the receiver, waiting for Send()
		receiver->state = STATE_SEND_BLOCK;
    
    // return for now; Send() fills in the actual return value
    return SUCCESS;
	} else {
    // the receiver has a non-empty send queue
		// pop the next sender off the send queue
		td *sender = PqPopFront(&receiver->sendQ);
		*tid = sender->id;

		// copy the sent message to the receiver
		int to_copy = Min(sender->msg_len, len);
		MemCopy(msg, sender->msg, to_copy);
		
		// block the sender, waiting for Reply()
		sender->state = STATE_REPLY_BLOCK;

    if (sender->msg_len > len)
      return ERROR_MSG_TRUNCATED;
    else
      return sender->msg_len;
	}
}

int SysReply(sched *s, td *task, int tid, char *reply, int len) {	
	// error checking
	if (reply == NULL && len > 0)
		return ERROR_NULL_POINTER;
	
  if (tid < 0 || tid >= s->next_task_id)
		return ERROR_INVALID_TID;
		
	td *sender = &s->tasks[tid];

  if ( sender->state != STATE_REPLY_BLOCK )
    return ERROR_NOT_REPLY_BLOCK;
	
	// wake up the sender and add to ready queue
	sender->state = STATE_READY;
	PqPushBack(&s->queues[sender->pri], sender);
	
  if (len > sender->reply_len) {
    // not enough space to copy reply to sender
    sender->ret = ERROR_TX_INCOMPLETE;
    return ERROR_NOT_ENOUGH_SPACE;
  } else {
    // copy the reply to sender
	  MemCopy(sender->reply, reply, len);

    sender->ret = len;
    return SUCCESS;
  }	
}

int SysAwaitEvent(sched *s, td *task, int eventid, char *event, int eventlen) {
  if (event == NULL && eventlen > 0)
    return ERROR_NULL_POINTER;
  if (eventid <= EVENT_NONE || eventid >= EVENT_MAX)
    return ERROR_INVALID_EVENTID;

  s->waiters[eventid] = task->id;
  task->event = event;
  task->event_len = eventlen;
  task->state = STATE_EVENT_BLOCK;
  
  return SUCCESS;
}

int SysParamTest(sched *s, td *task, int a, int b, int c, int d, int e) {
  if (a != 0x12 || b != 0x34 || c != 0x56 || d != 0x78 || e != 0x9A) {
    bwprintf(COM1, "a = %x\r\nb = %x\r\nc = %x\r\nd = %x\r\ne = %x\r\n", a, b, c, d, e);
    // bwprintf(COM2, "spsr = 0x%x\r\n", task->spsr);
    return -1;
  }
  return 0xff;
}
